
import sensor, image, time,pyb
from pyb import Pin, Timer,UART,Pin
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.skip_frames(20)
sensor.set_auto_exposure(False, exposure_us=2400)
sensor.set_auto_whitebal(False)
clock = time.clock()

pin1 = Pin('P1', Pin.IN, Pin.PULL_UP)   #外部中断,暂停按钮
pin2 = Pin('P2', Pin.IN, Pin.PULL_UP)   #开始程序
pin3 = Pin('P3', Pin.OUT_PP, Pin.PULL_NONE)  #声光


red_threshold = (30, 100, 15, 127, -40, 127)  # 红色激光笔的颜色阈值
green_threshold = (9, 86, 10, 71, -2, 66)    # 绿色十字的颜色阈值
text_Aend_x=0
text_Bend_y=0

redx=0
redy=0

Ax_n=0
center=[68,82]

uart = UART(3, 9600)

def callback_PIN1(line): #中断函数
    while(1):
        #print(pin2.value())
        print(pin1.value())
        if pin2.value()==0:
            time.sleep_ms(20)
            if pin2.value()==0:
                break
extint = pyb.ExtInt(pin1, pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, callback_PIN1)
def cmd_servo_move(num,run_time,servo_num,angle,servo_num1=0,angle1=0,servo_num2=0,angle2=0,servo_num3=0,angle3=0,servo_num4=0,angle4=0,servo_num5=0,angle5=0,servo_num6=0,angle6=0):
    head =0x55
    data_length = num*3+5
    cmd = 0x03
    if num == 1:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 2:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 3:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 4:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256, servo_num3, angle3%256 , angle3//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 5:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256, servo_num3, angle3%256 , angle3//256, servo_num4, angle4%256 , angle4//256])
        uart.write(data)
        time.sleep_ms(run_time)
    elif num == 6:
        data = bytearray([head , head , data_length, cmd , num, run_time%256 , run_time//256 , servo_num , angle%256 , angle//256, servo_num1 , angle1%256 , angle1//256, servo_num2 , angle2%256 , angle2//256, servo_num3, angle3%256 , angle3//256, servo_num4, angle4%256 , angle4//256, servo_num5, angle5%256 , angle5//256])
        uart.write(data)
        time.sleep_ms(run_time)


def Turn(Px,Py):

    dx=round((155/178)*(Px-46))
    Sx=1855-dx#左右方向加减相反，变成加
    dy=round((120/211)*(Py-6))
    Sy=1380+dy
    return Sx,Sy

def reset_duoji():
    cmd_servo_move(3,500,5,2000,7,1395)#5,水平，6，竖直

def find_blob_center(threshold):
    blobs = img.find_blobs([threshold])
    if blobs:
        b = blobs[0]
        cx = b.cx()
        cy = b.cy()
        return cx, cy
    return None, None
def red_xy():

    clock.tick()
    img = sensor.snapshot().lens_corr(strength=1.6, zoom=1.0)
    #img.draw_rectangle(left_roi)

    # 检测红色激光笔的位置
    laser_x, laser_y = find_blob_center(red_threshold)


    if laser_x is not None and laser_y is not None:
        # 绘制绿色十字来表示激光笔的位置
        img.draw_cross(laser_x, laser_y, color=(0, 255, 128))

    # 显示图像
    img = img.to_rgb565()
    #print(laser_x,laser_y)              # Note: OpenMV Cam runs about half as fast when connected
    return (laser_x,laser_y)

while(True):
    pin3.value(False)
    Rx=1750
    Ry=1440
    last_str=[1750,1440]
    num=0
    while(1):
        #reset_duoji()
        pin3.value(False)
        clock.tick()
        img = sensor.snapshot().lens_corr(strength=1.6, zoom=1.0)
        #cmd_servo_move(3,300,5,1775,7,1475)#5,水平，6，竖直
        list=red_xy()
        if list[0] is not None and list[1] is not None:
            str=Turn(list[0],list[1])
            #print(str)
            if(str[0]>1750+4):
                Rx+=3
            elif(str[0]<1750-4):
                Rx-=3
            if(str[1]>1455+5):
                Ry+=3
            elif(str[1]<1455-5):
                Ry-=3
            elif(str[0]<=1750+4 and str[0]>=1750-4 and str[1]<=1455+5 and str[1]>=1455-5):
                pin3.value(True)
            last_str[0]=str[0]
            last_str[1]=str[1]
            num=0
                #time.sleep_ms(1000)
        else:
            if(last_str[0]>1750+4 and num<=10):
                Rx+=3
                num+=1
            elif(last_str[0]<1750-4 and num<=10):
                Rx-=3
                num+=1
            if(last_str[1]>1455+5 and num<=10):
                Ry+=3
                num+=1
            elif(last_str[1]<1455-5 and num<=10):
                Ry-=3
                num+=1
        cmd_servo_move(3,1,5,Rx,7,Ry)#左右方向加减相反'''
